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ABSTRACT 

This paper presents a method of optimal filter 
design for sampled data systems, based on the theory 
originally developed by R. E. Kalman. The first half 
of the paper deals with the theoretical development 
of mathematical models for linear, discrete dynamic 
processes and the optimal filter equations for such 
processes. ‘The latter half discusses digital pro- 
gramming techniques for optimal filter design followed 


by two illustrative examples. 
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PREFACE 


During the past several decades a fair amount of 
theoretical effort has been devoted to the study of 
problems which are of a statistical nature. Not the 
least important is the class of problems in SGrmnneae 
tion and control which involves the separation of random 
signals from random noise, or the estimation of the 
states of a dynamic process based on noisy observations 
of a few of these states. 

In several papers written since 1960, R. E. 
Kalman developed a theoretical approach for optimization 
of filters for the above mentioned class of problems. 
The theory is not alleembracing in that certain con- 
ditions must be satisfied before his technique can be 
employed. 

The intention of this paper is to present a method 
for optimal filter design for sampled data systems, 
based on Kalman's approach. The first half of the 
paper deals with the theoretical development of mathe- 


matical model parameters for linear dynamic processes 


iil 





and the optimal filter equations for such processes. 
The latter half discusses digital programming techniques 
for optimal filter design followed by two illustrative 


examples. 
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MODELS FOR RANDOM PROCESSES 


Before one can hope to achieve any amount of 
effective filtering, it is necessary that a fair amount of 
knowledge, about the physical phenomena to be observed, 
is known. For instance, if a sine wave buried in noise, 
is to be recovered, an apriori knowledge of the signal, 
i.e. frequency of the sine wave, is necessary. In addi- 
tion if the statistics of the noise are known then 
optimum filtering can be achieved. It therefore becomes 
necessary to make a study of the message (signal) 
process before the construction of a filter is attempted. 
To maintain generality we will henceforth only consider 
random signals with the added stipulation that these 
signals are produced by linear dynamic systems excited 


by white noise. 


lel LINEAR DYNAMIC SYSTEMS (CONTINUOUS 
Aaa Ae) 


Since we are concerned only with linear dynamic 
systems a brief review of linear differential equations is 


in order. 





= 
“so — 


A first order differential equation 


dt aox au (1.1) 


——— 


at 


has a solution (see Appendix I) 
xe = eZ, + fer (t-Dugdr 1-2) 


fm t 
where © xX is the homogenous solution and [e@ Dugdy 
a 


is the particular solution. 
Consider now a set of first order differential 


equations, which define a linear dynamical system: 
ds = $ lxj,u) 3) 
C 


or in vector notation 


x =FX+dH) ul (eh) 


= 


where x and u are 1 xn column vectors and F and D 
are n x n matrices. 
The solution (see Appendix I) to this set of 


equations is: 


XH) = ef) ee + feof PE? pup u(t) dy (1.5) 
”) 





Or it May be wriececen 


: 
XG) = O(t,t.) X(t) + f $ (t,7) 0 an aT (1.6) 
to 


By définicion we call the vector x Ene staecic. 
the system and u the input or control function. 
Since all states (x;) may not be observable we 


define the output of the system to be 


yl) = He) x © 67) 


where y(t) is a p vector and H(t) is a pxn matrix. 
If allstates were observable then H would be equal to 
the identity matrix I. 

We can now represent the system in matrix block 


diagram form as shown in Fig. lel. 


ul) ~—— ) Xx) x) _ y&) 
De ee Jf SSH |S 








Fig. lel Matrix block diagram of a linear dynamic system. 
The integrator in Fig. lel actually represents n 
integrators, one for each state of the system, while 


E(t) shows how the outputs of the integrators are fed 
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back to the inputs of the various integrators. Perhaps 
2 look at a simple 2—state system at this time might 
clarify Fig. lel. 

Given the linear dynamic system of Fiig. lez, deter- 
Mine EG pee arid. G4 te). 


u@ _ 


| 


x(t) xi yt) 









Fig. l2 A Restate system 
We can immediately write down the equations for 


the system as: 
=, = eee) (1.8) 
Hae) == baer xX, + ult) (1.9) 
and our observable state(s) 


ud) = x, (1.10) 


It is immediately obvious that 


| , D&= ie and Ho) = [1 0} 


~ o¢ O 


O 
F(d) = 
~P 


thus giving the vector differential equation 


| ee nr} ni (1.11) 
iy | = 7 : 
j Ae ~-B  -& lo) ! U 


rh 


Nena ee 





and y@ = [1 oe 


Xe (1.12) 


1-2 LINEAR DYNAMIC SYSTEMS (DISCRETE- 
TIME). 


If we specify the equations of a linear dynamic 
system in the form of difference equations then they 
are easily mechanized on present day digital computers. 
With this in mind the scope of this paper will be 
directed towards discrete-time situations. 

In (1.6) we see that the continuous-time solution 


to a linear dynamic system is: 


x® = O(t,t) x(t) + fo é,) Dinuwdr (1.6) 


If u(7) is held constant over the interval of 


integration then we obtain: 


x(t) = @ (t,t) X(t.) + A (t,to) Ube) (1.13) 

where — ia s 
© 

A (t,t) = f G lt,7) D@ dr (1.1) 


or more conveniently 


alas tl) = — (t +1, t) x) + A (tt1,t) Ul) (eis) 





In (1.15) we assume a sampling period of one time 


unit. A block diagram of the linear discrete-dynamic 


system is shown in Fig. 1-3. 


u(e tH) luni ee) (+) 
a(t] ; 3 


O(t+1,t) 


Fig. l-3 Matrix block diagram of a linear discrete- 








dynamic system. 


l-3 DETERMINATION OF MODEL PARAMETERS. 
ite mat rion O(¢+i,8 occuring in 1.6) anda Ulicy mis 


called the TRANSITION matrix and has the following 


properties: 
O(to,t.) = I (Identity Matrix) (1.16) 
@ (te, th) (tir te) = D (te y te) (1.17) 
ra Ree to) = F(t) 6 (t, to) (1.18) 


(1416) and (1217) are fairl, cbhvicus and sole) mic 
obtained by setting u(t) to zero and differentiating 
(1.6). These properties can be useful in checking the 
accuracy of analytic expressions for the 0) matrix. 

If the F matrix is constant then the transition 


ba 
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matrix elements depend only on the time difference 


t—to and can be calcwlated from the following expres- 


sion: 


O(er)e e922 [ecto] Ji! as) 

aye 

A second (and easier) method for obtaining fie, to) 
is by the use of signal flow techniques and the eis 
cation of unit impulses to the input of selected 
integrators. ‘This method will be demonstrated else~ 
where in this paper. 

The A matrix may be obtained by performing the 
integration in (1.14) or by the second method mentioned 
above for the ) Maer. 
let} THE GAUSS-MARKOV PROPERTY. 

A large number of physical phenomena possess the 
Markov property. Basically it means that the best 
estimate of a future state of a process can be made 
without the knowledge of ail the past history of the 
process. Ina strict sense it implies that the best 
estimate of a future state can be derived from the 


last observation of the states. A very trivial example 





would be the motion of a particle with a constant velocity 
vector. Given the best estimate of the present position 
and velocity of the particle one can formulate a best 
estimate of position and velocity for any time in the 
future. In fact the output from any linear dynamic 
system is Markovian. If u(t) is set equal to zero in 


(1.15) then this property may be expressed mathematically 


aSs 


SOCE)) = P(t +, t) 2) (1.20) 


If u(t) is a gaussian random vector then the sequence 
of random vectors ...x(t=-l1), x(t/l), ... generated by 
(1.15) is known as a gauss-Markov_ sequence. The stipu- 
lation that u(t) is gaussian implies that the sequence ..., 
u(tel), u(t), u(tAl), ... are normally distributed random 
vectors such that the cross-variance matrix: 


COV f u(t) » U (t2)/ = ©, for t,#T; (lea 


i.e. u(t}) and u(t2) are independent. In addition the 
random vectors are completely defined by specifying their 
first and second order moments. i.e. E(u(t)) and 


E(u(t).u(t) ). . For the purposes of this paper u(t) 
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will be assumed to have zero mean; 
(eae SR (1) eae for all t (1.22) 
E(u(t).u(t) ) is called the auto-covariance matrix 


of the vector u(t) and will be denoted by U(t), i.e: 


pik iis) o Wee If = Ue (1.23) 


{1 


or 


cov [ ult) | Ute) 


Considering now the state of a process, we assume 
that the initial state, x(t >), is a gaussian random 
variable of zero mean and arbitrary variance. By 
repeated application of (1.15), we see that future states, 
woe, x(tel), x(t), x(tfl), ... will also be gaussian 
random variables, since they are obtained by linear com- 
binations of gaussian random variables. 

In probability terminology we may now define the 
Gauss-Markov property. Since u(t,) and u(tp) are 
independent for t,; # tp, then the conditional probability 
distribution of x(t) is dependent only on the previous 
state, i.e: 

PU) SW | oc t=), 22) ce 3) ee 


= P(x) <y |] X(t-1)) 





Where ¥] is an arbitrary vector. 
I=5. PHE COMPLETE MOb@et 

In Fig. l-3 the observable output vector Bt) cannot 
be measured with infinite accuracy and therefore to com- 
plete the model for random processes (with previously 
mentioned restrictions), a source of measurement noise 
must be added. This is illustrated in Fig. l-4 where v(t) 


is a noise vector having the same dimensionality 


| v (t) 
ul) x (er) [o> x (¢) y (t) z(t) 
NG = Oe iy ) HCE) ans 


Fig. l-l Model for random processes generated by discrete- 
time linear dynamic systems. 
as y(t). v(t) is white noise (gaussian), which we assume 


to have zero mean with arbitrary variance: 
= ages ue (1.25) 
Efy®-vw)'J]=covLv@] = R® (1.26) 


In addition we specify that v(t) and v(ts) are indepen- 
dence £Or ti *t2, i.e; 


COU | wiki) Uilts)| = Oe means (1.27) 
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The output of our model is therefore z(t), which 
contains the observable vector y(t) corrupted by additive 


white noise, v(t). 


Z(t) = y(t) F vf) ieee) 


ll 





CHAR wri iL 


of FL oA VANS ie ee 


gal DEFINITION OF THEVEIL 2 eR NG ee Wee ae) 

In Chapter I, a model of a linear dynamic system, 
excited by white noise, was developed. The purpose of 
the Kalman filter is to give a best estimate of all states 
of the system, based on noisy observations of the 
observable states. Since the system is linear we may 


write 


ue) = X(t) + G4) [2 (4) ~ 2(t)] (2.1) 


where x*(t) is the best estimate of x(t), based on the 
ererents ObSeimvatlomeZiaty) 

x(t) is the best estimate of x(t), based on the 
previous observation z(t-l), 

Z(t) is the best estimate of z(t), based on the 
previous observation z(t-l), and, 

G(t) is an nxp gain matrix, the magnitude of its 
elements being indicative of the amount of information 


Pecricd ine Zt) 


ee 





To solve the filtering problem, the filter must 
therefore determine values for the three unknowns on 
the right hand side of (2.1), namely x(t), z(t), and G(t). 
p=e. SOLUDION OF THE BPIibTRRING 2] -eeeew. 
Since we assume a complete knowledge of the 
dynamics of the system, computation of X(t) and a(t) is 
quite eee 


E [x(e)} 2¢e-)] 
O (ty t-1) 2% (e-1) + At ELued] (2.2) 


eG) 


Uf 


if 


however since Flu (t)] = O for all t then 


Xt) = Olt, t-1) 2S e-1) (2.3) 


In the model we saw that 


Ze) = ut) Fe) a) 
= H(t) x(t) + Ut) 
now z(t) = El =) |Z (t-1)] 


Hit) Efe) {e(e-0] + ELyW] 
= fey oete) os oe 
since E[vye)] = 6) 


it 
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men 


ff 


\ 


A A, : 

Having now developed expressions for x(t). and z(t), 
a matrix block diagram of the filter (see Fig. 2-1) can 
be produced. ‘The only unknown yet to be calculated is 


the optimal gain matrix G(t). Before approaching this 


CRG ater a criterion for optimal must be specified. 


\ 


Og — 


GC) ee 
; a 1 hr 
| : watt 


Pigs e-) LAE OFT iMate ihe. 


z (t) 


ea. TC ys i‘ 


Ape: xt-1,t-1) 


dela 





SSO LT (a) eee 


The criterion used is that we wish to find G(t) 
such that the loss function 

Lz E [cox ct)-20%e)"(2@-xX)] is minimized. (2.5) 
That is to say that the sum of the variances of the 
errors associated with the estimate of the individual 
states is minimized. Because the errors are gaussian 
it can be shown (ref. 1) that this criterion will in fact 
produce an optimal gain matrix. 

A number of different derivations for G(t) are 
available in the literature. For the most part these 
derivations are mathematically rigorous and somewhat 


complex. Perhaps the easiest one to follow is a semi- 


Na 


» 
e 
xy 





heuristic approach used by Schmidt, fyb. We wish to 
minimige the scalar loss function defined by (2.5). Note 
ehat om 

tl 


E Lee - 208) ( 2x] = Trace EL(x- 2 2e-X4)) (2.6) 
where the Trace operator denotes the sum of the main 
diagonal elements, and (tjris lert out to avoid sunmeces= 
sary clutter. 

Expanding the covariance matrix in (2.6), using 


(2.1), we obtain 
Eb “20-268 9) = EL (x-2)-6 (g-8)) [te-2) -G(2-2]]" 4 
= EL(xz-22t-2)7) - Elo (2-2-8 
~E[( 2 )(z-2)'67] + Efe @-2z-2y'6"/ 
but B= AX ty, and, @= HR 
Substituting for z and g, and noting that Eliz-%)v" 20 
we obtain 
EL x2 2-28)" ] © E [ce Ee 2)" - ELGH Ce-2e2)"] 
MELZ-2MeR HG] ve 2 e ED iho 
FE{G(H(x-2)(x-X% Tue vue’ | 
=P-GHP -PH'G"+G(HPH™R)G" (2.7) 
where P= E[(x-R)(x-Z)7] and R= ELy-v™] . 





We now wish to find an expression for G such that 
the trace of (2.7) is a minimum. Since the terms in 
(2.7) are matrices this could become a very arduous task. 
Let us consider for a moment that (2.7) is a scalar 
expression (i.e: the matrices are lxl in dimension), thus 


reducing the right hand side to: 
P~2G6PH'+G (HPH' +R) 


We now differentiate with respect to G and set the 
resultant to zero obtaining 
~2PH7 + 2(HPH'+R)G =O 
or §=G= PH? (HPH™ +R)" (2.8) 


It can now be shown that (2.8) will in general 


os 


provide the optimum gain matrix by letting 
C > G- PH (HPHT+R)7' 


or G2C#t PHTCHPHT+R)! (2.9) 


Simple substitution of (2.9) for Gin (2.7) will 
reveal that the trace of (2.7) will be a minimum for 


C:0. Thus (2.8) provides us with the optimum gain 
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equation. 

Corbining equations (2.7) and (2.8) results in an 
expression for the covariance matrix of the error in the 
filter's estimate of the states of the system; 

EL -0*)(20-20")7] = D-PHT(NPH RYH - PHT[PHT(HPHTeR)' |” 

0" +[PH7(HPHT+R) JT HPHT+ RILPHT(HPH+R)]” 

| - Plt) = P(E HE) (ite) PE) He) +RO), HOPED (2.10) 

ea ~ P-GNP 2(t- GH)P 

In order to complete the filtering problem a recur= 
sive relation for the conditional covariance matrix, 
P(t,t-1), must be derived. 

Recall that 

P(tyt-1) 2 EC x) - Ze e-9) (x (a) - Xe)" | 
bur (4) = O (t,t-/IX(t-1) + A(t, t-YUG-N 
and X(ev-8) = @ (t,e-l) A (t-4,t-)} 


+ p(t tt) sESLO Ue t-0(x ey x*leye-s)) + alte) uy le-d]- ve 
fe Cee (eri, ¢-i)) O'(t , t+) + Ween) Ae ty} 
2 O(t,t-ELCX(e-) -xVese-y(xce-y-2le-nty fe Ze 
O' (e,t-)) + A(e,t-1) Ely Ce-i)ufle-)] A (t,t+) 2-1) 


bb) 6 E(k AO [Gea elael]e 





since the expected values of the cross=—-product terms are 
Zero. 

¢\ 
Define Q(t) = A(t. t-NE[u(t-)-u'(e-J]A (t,t-1) (2018) 


— 
Combining (2.10), (2-ll) 5; and"(2.l2)) wewobeam 


‘ - 
P(tyt-s) = Ot, e-1) [Pee = P(e“) H'(t-! (HCt-JPCe- Het R(ey)- 


—— ine 


H(t) Plt-)] d (t,t) + O(t) (206) 


Equation (2.13) is calledthe variance equation which 
denotes the covariance of the error between the actual 
states x(t), and the predicted states a(t,t-l). : 

Since (2.13) is recursive, an initial covariance matrix, 
P(t,), must be specified, and, since we assume that x(t,) 
is gaussian, with zero mean, our best estimate of x(t.) is 


zero. Hence 


P(t.) = E fiz (te)+xX'(t.)] (2614) 


In determining P(t,), one will often find that the 
elements off the main diagonal will be zero, that is to 
Say that the individual initial states are independent of 
one another. ‘To illustrate this, perhaps a simple example 


will be helpful. Det us suppose that we are going to make 
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observations of the position of a particle in motion, and 
assume that the particle's velocity is constant but unknown. 
The two states of the system are position and velocity. 
Any knowledge one might have about one of the initial 
states will in no way assist in determining the other initial 
state. Hence the two states are initially independent inso-= 
far as the observer is concerned. However, as more 
observations are made, the two states build up a depend= 
ency and the off-diagonal elements in P(t), generated by 
(2.13); become non-zero. 
a-3 SUMMARY OF FILTER EQUATIONS. 

For convenience the equations for the optimal filter 
are grouped below: 
Ar = O(t, t-i)|Ple-1)- G(t-i) Fi(t- DPE-N Ore, <1) eet) ae 
6() = Pt) Ace) [HC PCevHT(e) + Ree)] : II 





de, -1) = O(t,t-1) X*(t-1, 0-1) | sste 
(4, t-1) = H(t) Xe, t-i) ~ : IV 
Xtt,t) = B(tye-) + 6) L2@ - Z at-u] Vv 


Ls oh AAL ) OF 2A 
q 7 


i poe 93 "I —. LY 


¢ ~ Purr) vent +e | 





Big Ved Aline) Sb itat 


Pie Boe) BS GaN 


B=) SOME PRELIMINARY CONS IDERAYTIGNe. 

The equations for the optimal filter were derived in 
Chapter II, and are summarized in Section 2-3. Design 
of the optimal filter consists mainly of writing a suitable 
computer program to carry out the calculations indicated 
by the filter equations. I through V. . The input to the 
filter.is the noisy observation vector, z(t), and the out- 
put is the best estimate of the system state vector, 
x*(t). 

One of the chief problems in carring out the filter | 
computations is the determination of the inverse of the 
matrix found in equation II. If this matrix is singular, 
then the inverse does not exist. One must then resort 
mOrtcne calculation of a Seeucae in enses The manner in 
which the pseudo-inverse is determined is shown in[2]. 
and will not be discussed further here. In either case 
the time required for inverse computation is relatively 


high, the time being roughly proportional to the cube of 


£0 





the dimensionality of the matrix. On present day com- 
puters several seconds of computation time may be 
necessary to determine the inverse of a x) matrix. 

This being the case, one can easily see that the sampling 
Rabe maybe rodVversely ahwect ear. in CUeCLeHOremDelioves 
one to make a close study of this matrix to see if com= 
putation time can be reduced. 

To illustrate, let us assume we are going to track an 
object in space, receiving position information only. The 
observable states are range, bearing, and elevation (R,®6, 
0), and we wish to determine best estimates of these 
states along with their derivatives (R, S, ). Our state 


vector would then be set up as follows: 


lee 3c] 
R eres 
S BB 
x(t) ) Si =| xi 


Q x5 
iO fe ps 


The H matrix would become 


100000 


H(t) = | 001000 


O000%0 


a) 





and hence the matrix ( HPH + R ) would be 3x3 in 
dimension. 
Recalling that P(t) is symmetrical, computational 


reduction might be possible if we can assume 


Ef (Xe (t) ~ 2%; (t,t-1))(Xigo (t) ate (¢,t-1)) <O 


for l= 1,4 
and further that R(t) has non-zero elements along the 
main diagonal only. Making the above assumptions the P 


and R matrices would become 


pll pl2 0 O O O 
p2l p22 0 0 O O 
BAe) a 0 0 pB3 8 6pay 0 0 
0 0 p43 = ph 0 0 
O O O O p55 p56 
is O O 0 O p65 po6 
and R(t) = rll O O 
O E22 O 
O O Goo 
We would then find that 
aw O O 
eae ee 
= ial O uy O 
( HEH +R ) = p33 + r22 
1 
[os eee. 


ee 





since the inverse of a matrix, having non-zero elements 
along the main diagonal only, is found simply by inverting 
the diagonal elements. 

Suppose now that rate information is also available 
so that measurements of all six states are made. If, 
in addition to the above assumptions, we can assume that 
the cross-variance elements in the P matrix, involving 


the even subscripted states, are also zero, then 


‘a =I 
at eple| OeeeOn Ome 
[pal a22z}_0 0 0 0 
O O° fa33 p34l. 0 0 
(HPH#R)-+ =| 0 0 Re aii] O O_., 
OURO =O Oy fasomDS 
O 0 0 O |p65 a6é6 


L 
where aii = pii + rii, i = 1,6 

thus reducing computational time by at least 63/3x2 or 
9 times. 

A further reduction might be realized in the given 
example by the use of three filters doing 2x2 matrix 
manipulations as opposed to one filter computing at the 
6x6 level. 


Another consideration is the time lag between input 


and output. In real time situations this could be of the 


23 





utmost importance. A. glance at the filter equations, I-V, 
indicates that if all five equations are computed after the 
input, z(t), has been received, then a considerable time 
lag could ensue before the output is generated. On the 
other hand, if the transition matrix, O(t,tel) is known 
at time tel, then equations I, II, III, and IV can be 
computed prior to receiving the input. The time delay in 
this case would only involve the time taken to compute 

V, which might be in the order of micro-seconds. 
S-eeelloiterR POR A STATIONARY PROCHSS. 

It is to be noted that equations I and II do not 
involve the observations, z(t). If the process, which we 
are trying to observe, is stationary, ies, lah ® (OP eck 
rR are constant matrices, then it will be found that the 
optimal gain matrix, G, will stabilize to a constant value. 
This matrix could then be precomputed (prior to any 
observations), and the filter would be reduced to the 
relatively simple calculations indicated by III, IV, and V. 

A digital program, which computes the optimum 
gain matrix for a stationary dynamic process, has been 


written in FORTRAN, and is found in Append it ee 


Sony 





is completely general and can handle any system with up to 
twelve states. It is written as a subroutine to eliminate 
possible conflict in the naming of variables. . Essentially 
the subroutine carries out the iterations indicated by I 
and if until such time that no element of the gain matrix 
changes from its previous value by an amount more than 
00001. If higher accuracy is desired, this constant can 
be easily changed to the degree of accuracy required. 
Further description on the usage of this program is found 
in the Appendix. 

f=—3 LHE GENERAL FILTER. 

In the general case, non-Stationarity is assumed. 
Appendix III contains two programs, the first of which 
performs the computations indicated by the five filter 
equations after each observation. The second programme 
allows for the case when the transition matrix is known 
before an observation is eee and hence reduces the 
time lag (previously discussed) between out put and input. 
Further discription of the usage of these programs is 


contained in the Appendix. 
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This section of the paper will deal with two illustra- 
tive examples. The aim of the first example is primarily 
tutorial. A thorough discussion of the model will be given, 
followed by the design of an optimal filter. The second 
example will deal with the track smoothing of an anti-ship 
missile. A mathematical model of the missilein flight will 
be set up and a filter designed for optimum track amcouns 
ing. 

PAM LE I: 

Given the transfer function for a low pass filter in 
Fig. 1, a)determine all mathematical model parameters , 
and, b) design an eeiagi filter which will give a best 
estimate of the states in the filter. Assume that the 
excitation at the input (u(t)), and the additive noise 


eat) ) at the output are gaussian and stationary. 


Fig. 4=1 Low pass filter of Example I. 
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Our first step is to convert Fig. 4-1 to a more 


convenient form for analysis as shown in Fig. =. 





Fig. y-2 Signal flow graph for Example I 


From Chapter I, equations (1.8) through (1.12), we see 


chat x(t) = Fx(t) + Du(t) (ie) 
or 
Seam O X ,(t) O G o 
ae ie 
Xo(b) —2 -3] | X.@ O u(t) 


We wish now to find the solution to ().1) in the 
form given by (1.15). To do so we must assume that 
u(t) is piece-wise constant. 

The 0) and A matrics will be obtained by using signal 
flow techniques and applying unit impulses at selected 
locations in the diagram as illustrated in Fig. hea. | 


From Fig. hee we see that 


X, (5) XM 
ad (S) = rol 52 
a Xo (S) 2 (3) 
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O 20 FIGS, 
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ONG Ee stilts) 2: _s+3 
5, | pos, ae $7435 +2 
S S 
O12 (5) = x, (8 eae ee = eo 
oo 
QM, ,(S) = ze(s) = st Een tl 
8 It 24% s° +35 +2 
pa 
() (Ses Aiggl Ses Bo ee 
ree Se ltr S Stes tee 
ie 53 lt 32+ SSeS re), 
2L2.(s) see 
ins (S) = =f =s 2 = 
i 03 proartz s*¢3s+2 


Taking the inverse of the above and letting the sample 


interval be — we find 
= - 
ME Se 2T 
() (t+7,t) = 
~ =|) an 
2C€ —2e& 


eh ome 


(4.2) 


ee eng 
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JA (t+T.t) = (14-03) 


he solution is mew im the form.on 41.15) 


X (tet) = O(tent) xe) + Alt+7 t) u@ (1.15) 


and the mathematical model is shown in Fig. 4-3, 


y(t) 
2%) 






Figure 4-3 Mathematical model for Example I 

where H(t) is obviously [1 ol, Since only one state, x; , is 

observed. 

b) Design of optimal filter 

We recall that optimum filtering is based upon a 

knowledge of the statistics of u(t) and v(t), and there- 
ce T- 

fore we assume that Blu(t)-u (t)} and E[v(t)-v (t)Jare 

part of the problem statement. We now calculate the 

covariance matrix 


OC eo =e +T,t) E [ ue) - W(t) ]A™ (tr) t) (2, 0) 


RO 





and R(t) = EL ue)-v'e)] = Aa (4.5) 


The only remaining task is to select an initial 
covariance matrix, P(to), for our best estimates of 
the initial states of the model. ‘The selection will depend 
to a great extent on a good knowledge of the problem. 
In this instance the best selection would probably be the 
main diagonal of Q(t) (previously calculated)} with the 
off-diagonal terms set to zero. The off-diagonal terms 
are zero initially since knowledge of x(t.) (at the first 
measurement) will in no way provide any information 
Bout scrote) ie X}(t.) and Xo5(t,) are uncorrelated inso= 
far as the observer is concerned. | 

Since the system under study is stationary, the 
optimum gain matrix may be pre-determined. ‘This 
Preailc the use of SUBROUTINE CONF IL in Appendix 
II. A sample period of 0.1 secs. is used. Using (4.3) 
and (4.4), we compute the covariance of the states, 
Q(t). The element q) in Q(t) is a measure of the 
expected signal power. By making rj, (measurement 


noise power) equal to various multiples of qy (including 
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zero) we are able to study the behaviour of the gain 
matrix as a function of noise-to-sSignal power. 

The curves in Figure 4-) indicate how the optimum 
gain matrix elements behave as noise-to-signal power is 
increased. With N/S = 0 we see that Gl is equal to 
unity. We expect this since there is no measurement 
noise and hence the best estimate of the observable 
state is the measurement 2(t) itself. However as the 
noise power is increased the gain element falls off and 
the filter starts to rely more on the predicted value 
anc less on the observed value. The matrix element G2 
also falls off in a similar fashion, with x",(t) becoming 
less dependent on z(t) as the relative noise power 
increases. 


HXAMPLE II 
Problem Statement - It is known that the enemy's 
main anti-ship weapon is an air-launched missile which is 
normally launched at a distance of 250 to 300 miles from 
arget. Artter launch, it climbs €o an altitude of 107007 


ft., attains a speed of approximately 1000 mph, and 


maintains this speed by use of a sustainer motor. When 


ie 





within 25 miles of the expected location of target a 
search device is switched on which pinpoints the pen cee 
and enables the missile to guide itself to target. A 
typical friendly ship at sea& is fitted with a mono-pulSe 
search radar system with a scan rate of 10 scans/min. 
The shio is fitted with a digital computer and target 
information is available to the computer. It is known 
that the probability distribution function of the 
accumulated error (in both x and y), is approximately. 
normal with zero mean and 2 mile standard deviation. 

. Data accumulated on similar missiles indicate that, 
due to erratic thrust developed by the missile motor and 7 
average atmospneric turbulence, the velocity of the : 
missile varies in a random fashion (approximately 
gaussian). The standard deviation of this randomness 
is agbout 2% of mean velocity. 

Design 5 filter which will determine best estimates 

f the missile's position and velocity. Assume that 
attack is equi-likely. from all directions. 
SOL Gd @n 


Our first sted is to set up a mathematical model 


Ee. 





which describes the dynamics of the missile, in the form 


% (t) = F(t) XE) + DCH) Ul) 


Considering only the component in the x direction we 


obtain Ux py 
x, (+) O 1] [2 C)) Cte - 

+ (4.6) 

DG a) lo = OF | X. Ct) Oo ft} Lue) 


1 


where x, is the position on the x axis 

x5 is the velocity component in the x direction 

A similar vector equation would describe the 
dynarnics in the y direction. 

As we have seen earlier the solution to the above 
equation is 


eee ee = 


X(t) = O(t,e) X(h) + Alt tut) — 7) 


nen en 


assuming u,(t)is piecewise constante 
We now must determine Q(tost,)- 
From (4.4) we obtain the model shown be ae SiS 5 


simple 1/s* plant. 
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U(t a oe = DERE 


Fig 3.5 Model for missle in Example II. 


The parameters Q and A are 


t2-%, 
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Q (t, y] ¢,) 
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‘oe (é,- t,)* 
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OQ t2 7h 


A (t, ? ti) 


Since we intend to sample at a rate of 10 times/ 
min. (scan rate of radar) then to ~.t; = 6 seconds. 


or [T= hrs ° 
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To complete the model we require values for H(t) and 


Vey 
(te) V mS 
1/ ke 
rom the problem statement 6 ee 
oP 
Ge 
pies ae (V 
2 (2) - Thy = re = li O 
and since we are observing position only 
H(t) = ft of 
Summarizing our model we have (Fig. 4.6) 
y tJ 
X(ttz55) 7) LEO PAZ 





Fie h.6 Model for Missle of Example II 


We are now ready to set up the filter. One 
requirement is the initial covariance matrix P(o). Since 
the missile can approach us from any direction with 
eoual probability our best estimate for the inital states 


is zero for both position and velocity, ie 


| 2, (0) [ 0 


aa (0) O 


Sift 


ie 
A. 





We therefore find that 
P(o) = ELX@)-Xx'Co)] 


To determine this we must have a knowledge about 
the detection capability of the radar. A standard search 
radar looking for an object at 40,000 ft. elevation may 
have &n initial detection density function, as shown in 


Figure 4.7, in all directions in the xey plane. 


Pad 
| 
O @f=200 Range (miles) 
FIG. 4.7 Probability density function for inital —— coh 
detection ae 


Since the missile may approach from any direction 
let us assume that the standard deviation when con= 
sidering the x direction only is 100 miles. Similarly let 
us assume the velocity component on the x axis has a 
standard deviation of 500 mph. .We then find that 

(.o0)2 O 


Po) = 


O (500)° 
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Our last remaining task is to select a value for We) 

to provide numbers for the Q(0) matrix. Our best 

initial estimate of this quantity would be the poo element 
Ww 

ine los er ce a 

Hience a W oo | 

V; (0) Se svole 

Subsequent values of Wt) could be calcwated by 
using x(t) ; 

We are now prepared to write a@ program for the 
optimal filter. 

Since the transition matrix is a constant but @ 
is variable (belna sa) funccion of Ve) the 2nd program of 
Appendix III would apply. 

We now summérize taking into account the y com- 
BOnone of direction. The flow chart for the filter 
calculations is shown in. annie OA Sg 
n {number of states) =k 
p (number of observables) = 2 


T (sample interval) = 1/600 hrs. 
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LINEAR DIPEERRENITIAL BFOUATIGNS 
Let us consider the solution of a Ist order dif-= 
ferential equation by the use of an integrating factor 


(p), to make an exact differential. 


Givens 

ax 

st = 

=~ tax U (1) 
Find: 

ee E(x; Wag c) 
SOlucion< 


Multiply (1) by p(t) and attempt to make the 
resulting equation an exact differential. We have 


dx 


ae + pax = pu 

or (2) 
d dp 
ap (Px) - at x + apx = pu 

and 


S- (px) = (S2- ap) x + pu (3) 
Considering the homogeneous part of the problem 
(i.e., u = 0), we can make (3) an exact differential 
by setting 


Lh 





ep 


ee ee (4) 


We may guess at a solution for (4) as 


end. by substitution in’ ()), verifyeanae If issa colmuion. 


Applying this to (3) gives 


c 
S= (poe**x) = (0) x + poe (6) 
integrating 
iS 
p, | e@*x - x, = f tulr) ar | (7) 
O 


Multiplying by eae gives 


ie 
x= eat x, + e7at | ENO?) avy (8) 
O 
or 
at eral tT) 
x=e” cin { pete u(7 2dr (9) 
re) 


The first part of (9) represents the homogeneous 


Solution while the latter part 


1c 
X, -| ee), BUG j@ aerou! 
Oo 
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represents the particular solution or convolution integral. 
Now let us consider a set of n of these Ist order 


ditferential equations 


qe Php u) i=], a elakenn 7 


(@3% 


= Es + Du (11) 


[he 


Waere x and wiare 1 x un Column vectors aud | jauc) 
are n x n matrices. Multiplying (l1) by the integrating 


factor p(t), (a row vector) we obtain, after some 


manipulation 
=O 
d A ee eco = 
ap. ( Px) (245r) s+ pu (12) 


As before we assume a solution for the adjoint variable, 
p(t) as 


5 SEE 
1) SOLE 


Substituting into (12) and integrating gives 
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oe t 
—— — te —_ : 
2) |e i Xo Xo — Se ET pu(r) ar | (13) 
oO 


Multiplying both sides by ae gives 


i: 
Pt F(t -7) 
x=e +) e Du (7) av (1) 


ften the above is expressed in the form 
ee) = Oem e.) x(cee Nemec) uco) amor 


where (ig the transition matrix or fundamental matrix, 
and /\ represents the distribution matrix with u(7) 


held constant at its value u(t,). 
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APE eNDE A it 
Pe BS eat ON Ae Yon Pi ae ee 

Eres > re 

The attached program can be used to determine the 
stabilized optimum gain matrix for the estimation of the 
states of a stationary process. 
USAGE: 
it Calling Sequence 

GALL CONE IE. (Ebay Grek, LR neo wee se erie 
G) 
ee Arguments 


ae I — initial covariance matrix of system states - 
Gimension (12 x 12) 


jor ©Q = covariance matrix of states due to gaussian 
excitation « dimension (l2 x 12) 


Cs R — covariance matrix of measurement noise e- 
dimension (J2 x 12) 


d. TR = transition matrix of process — 
Gimension (]2 x 12) 


Ce Fi = matrix which defines the observable states - 
dimension (l2 x 12) 


ft. KN =— number of states in the system-=- 
dimension (scaler) 
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eal 
a 
&. KP — number of observable states - 
dimension (scaler) 
h. KER = error indication (= 2 implies the inverse 
of a matrix could not be obtained) - 
dimension (1) 


- G — optimal gain matrix = 
Gimension (12 x 12) 


3. Accuracy: see Chapter III 
lh. Equipment Configuration: CDC 160, with FORTRAN 
S100 
5. Cautions to Users 

a. All arguments in the main program must be 
dimensioned the same as those in CONFIL. 

b. The main program sheuld contain an ERROR TEST 
Psce 2h) 


6. Flow chart showing Typical Usage (see Fig. 1141). 


LO 





initialize 
Ey Opie ye Ole 5. ICING ae 


CALI CCNEIL 









indicate 
error 


continue 


Fig. 11-1 Flow diagram showing typical usage 
ot CON ie 


50 





\§ 


(GSNNILNOD) TIANOD 3NILNOYSNS = Il XIQN3dd¥V 


| INNILNOD 9OT 
SS | LOT OL O09 Got / 
90T*SOT*SGOT (-X-T0000°)4I = 
((TOOOOO*+(FS1I9O)/(TOOOOO*+IFSI)L9O)— *T) 4SQy =x - 
ao dx#% =F 901 Ga 
NXA*T=I 90T OQ 


(1)9 4O NOILVZITIQ@VLS YoOd LSIL NIOIRg > 


(9*dA*dASNASTdW3L*dW3L)GONd WVvd Tot 
; LOTSOTLT*TOT (2-Y3y) JI 
(UBN*TdWILSZ2dW3L*TO000"* dy) dID3Y WIVd 
(ZdW3L 6 dS dNSysZdW3L) Gav WV 
(ZdWIL* dAtNASdAfdW3LSH) GOYd WV 


oe AGWAL SE GUSNAENASLHSd) GOYd VD vol 
~ . ae 
exe | 2A W a4 (L)9 3LVINDWD OL dOOT ZSAILVYSLI NIO39 a 
_ (LHSNXSdXSH) SNVYL 17VDs 
"O= (f° 1omcot 
. dX*t=r Zot Od 
NA*T=I Zot od 
(TIYINS (Z2T#ZTI Tdwal?2 
(ZTSZTISAWILE (ZTSZT) dW3L§ (2TESTIAHS (ZT SZTI LOSE ZTE STINE CZTEZTIONT 
CLS et) Tags (2TMCT IS b+ (ZT ZT ue 2 HST IHS (ce 1 Ott cl cla) NOlShanirao 
SS300Ud DIWVNAG YVINIT VLVO-G3IdWVS AUYVNOTLVIS > 
v JO S3LV1S 3HL JO NOTLWWILS3 TWHIldO 3HL YOd YRLIIA V NI > 
dasn 39 OL XIMLVW NIVO G3ZITIGVLS 3HL S3LNdWOD JNILNOYSNS STHL 3 


1 Uo ed * NASH al tO" d) TISNOD ANI nedgas 


VIANOD SNILNOYGNS 


: (OSJNNILNOD) II XIGN3ddV 





soe? 


(G3NNILNOD) I4NOD 3NILNoxNSns se 


(2TS2T IDS (2TS eT GS (2Tse2T) Vv NOISNSWIG 
(OISWSNSGSV) GAV SNILNOYGAS 


QN3 

(FSAIGRCAS EVV + UC © 155 eG 
WST = A TST OC) 

"O= Af Sis 

1*Tt=f TSt od 

N*T=I [S61 OQ 


(2T°2TID8 (21S cI Sst ci ciiy Neiswaiiig 
OS WEN Soo) Goud INI LNOYENS 


QN3 


? JANTLNOD 


NIVOV 


doo? 


70T OL O9 
(CS 1)9=( fF * toes 
dA*t=F 60T Oa 


NAS T= 60T OQ- 


(dS NASNASOSd)AQV 

Cd ® NANA NA CdWAal*sS TP edede 

(CdWALSNASNASNAS dGW3LSd) doud 
(dWSLSNASNASSLISNVIUL 

: (SLeNAS NA WS aay 


VV) 
V1V2 
Viv 
dave 
VIVO 


(PST) dW3L- =I ST) dWwal 


NAST=F 8oTt Oa 
NAfTt=I 8ot oa 


(dW STS Nae dA * NAY EM a Cagney 
C1d0*d At NAtNASO2 al) doddwihys 
NOTLVUSLI 3ANILNOD G3ZITIGVLS LON (1)9 
OLL Ogee 


ISdgi NOTIVZI Vey is adiee 


J 


Ott 


60T 


SOT 


Lot 


Il XIQN3ddV 


8. 


B, 


oY 





0S200 
07200 


O0€200. 


02200 
OTZ00 
00200 
O&T00 
O8TOO 
OLTOO 
O09TOO 
OSTOO 


O€TOO 
OZTOO 
OTTOO 
0OOTOO 
06000 
08000 
01000 
09000 
0S000 


07000 — 


0€000 


(GSNNILNOD) JIANOD> 3NILNOoYSNS ~I] 


Il XIQN3ddV° 


0€£0$*06(d3-((157)¥) 48a) 41 


a 


Z=(CFS dd)X 


(7° da) xX=(e* a 


(FSV) X=Z 


N*T=f ST Od 


Z=(f&dA)V 


CEC t Vas say, 


(FS) VeaZ 


N*1=f #7T Od 
Oc *O2*ET( dA~-1) aI 


JANI LNOD 
= da 


(( 18d VIASaV=Z 
CUSCTS TTC (18d) VV) 4SEV~-Z)4I 


N° a=A 24 30g 


0*0=2 
O=di 


: N*T=1 ve Od 


(Zine t ae tee 


O° 1 =(WE yx 
NS 1 = Ah 210G 
O°0=(r*I1)X 
N*t=f T OQ 
N° t=1 “tae@¢ 

NOISN3WIG 


(YSAtX*V*daeN) did4au SNI Enosgns 


O(a ay 


QN3 


{FSD 


W*T=f €ST Od 


N*T 


€gTt Od 


(2TS2T)d*(2T*2T)V NOISNAWIG 
(DtweNeVISNVUL SANT LNONGAS 


(C21) erie, 


GN3 


megs OS 


W*T=f 2¢6T OG 


N* T=] 


CGiINed 


O02 
ST 


7T 


el 
et 


It 


ect 





: ) (G3NNILNOD) TI4SNOD 3NTinoyans - = Il XIQN3dd¥ 


: QN3 
- 06700 : QN3 
08700 €=usN OS 
014700 8 NYNL3Y 
09700 2 4 . t=y30 
CS700 CILISTLIV/(S-CFSITLIXIS(FELIDX C4 
07700 — (COAX (NSLT TDI V+S=S 2% 
O€700 ; . N¢IidII=X 24 Od 
02700 - ItIll=Ttdli Ty 
OT700 Ev Sey [7 (N-IL)4I 
00700 | 0°0=S 
06€00 N‘T=f €% OG 
O8€00 I-T+N=I] 
01€00 N*T=I €7 OG OY 
09€00 , - JNNILNOD ¥%E 
OGE00 JNNILNOD 9€ 
OvEO0O (POT) X20 TLV YUE 8S) X= Oi eae ae 
OCE00 NS l= Gemoe 
0Z€00 . (CS VIVeHOTLVY-(E SI Va 8 ies 
OTE00 . _N€idl=f €€ Od 
COEO0O0 (Tet) V/07* 1) V0 § vee 
06200 ZESIESZEC( TSH) V)AI 
08z00 N&Tdl=_4 9€ OA 
azo © T+1=Td7 TE 


09200 VORUCS TC (N-)e eow 


Dy 





Eid oe ee 

GENERAL PILTERS 
PREAMBLE: Two programmes, in subroutine form, are 
contained in this appendix. The first programme carries 
out all filter computations after the input has been 
received at each sample instant. The time lag between 
oucput and input will therefore depend on the time taken 
for these computations. The second programme is 
designed for use when certain parameters are known (or 
at least a very good estimate of these parameters can 
be made) prior to receiving the input at each sample 
instant. <A prior knowledge of these parameters, namely 
TR(t,tel), H(t), R(t), Q(t) (defined below), enables a 
considerable reduction ef the above-mentioned time lag 
between output and input. This is achieved by perform- 
ing most of the filter computations prior to receiving 
the input. 
A. SUBROUTINE BESTX 
PURPOSE: This subroutine will provide an optimum 


estimate of the state vector for any sampled-data 


2D) 





lincar dynamic process (with twelve or less states) if 
both the process random excitation and corruptive 
measurement noise vectors have gaussian distributions. 
USAGE? 
i. Calling Sequence 

CALL BESTX (P, Q, R, TR, H, KN, KP, KER, 


Geer, 2, x) 


Qe  — initial covariance matrix of system states - 
dimension (J2 x 12) 


De Q ~~ covariance matrix of states due to gaussian 
excitation = 
Gimension (12 x 12) 


ee R = covariance matrix of measurement noise - 
Gimension (12 x 12) 


ad. TR ~ transition matrix of process = 
dimension (12 x 12) 


Ss H —- matrix which defines the observable states - 
dimension (12 x 12) 


fs KN = number of states in the system = 
dimension (scaler) 


remy Kee number of observables states — 
dimension (scaler) 


h. KER - error indication (=2 implies the inverse of a 
matrix could not be obtained) - 
dimension (1) 


ie G = optimal gain matrix = 
dimension (12 x 12) 


56 





je xP. predicted estimate of process state vector — 
dimension 


hee a--~ observation vector = 
Cimension (le x 12) 


le 2 —- optimal estimate of process state vector 
(generated by filter) - 
dimension (12 x 12) 
3. Equipment Configurations CDC 1604 with FORTRAN 
60. 
bay Ceecions (co -Wsere 
a. <All arguments in the main program must be 
dimensioned the Sameceas Luwse in vis mS io 
b. The main program should contain an PReOR tos 


(see. 2h.) 


c. See attached flow chart depicting typical usage. 
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Fig. 1ii-1 Flow diagram showing typical usage 
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8B. PUBROULINE GCA Dae 
PURPOSE: This subroutine will provide, for each 


saraple instant, am optimum gain matrix, and, the pre- 


— 


3) 


dicted values of the process and the observation state 
vectors, if the process is linear and the random 
excitation and corruptive measurement noise vectors 
have gaussian distributions. 
OS y20eashe 
Ine Calling Sequence 7 

CALL GANDPR (FP, OQsseRe oR, Fy x, XP, 2P, 

os 

KN, KP, KER, G) 
ee Arguments 


Ge FP - initial covariance matrix of system states = 
dimension (12 x 12) 

es Q- covariance matrix of states due to gaussian 
excitation — dimension (12 x 12) 


oe R— covariance matrix of measurement noise = 
dimension (12 x 12) . 


ds. TRe- transition matrix of process - 
Gimension (]2 x ]2) 


e. H =—- matrix which defines the observable states - 
dimension (12 x ]2) 
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le 


le. 


l. 


DO 


rN 


3. Bqupment 


optimal estimate of process state vector = 
dimension (12 x 12) 


predicted estimate of process state vector - 
dimension (12 x 12) 


predicted estimate of observable state 
vector = 
dimension (12 x l2) 


number of states in the system = 
dimension (scaler) 


mumber of observable states = 
dimension (scaler) 


error indication (= 2 implies the inverse 
of a matrix could not be obtained) - 


dimension (1) 


optimal gain matrix = 
Gimension (12 x 12) 


Configuration: CDC 160} with FORTRAN 


e 


Xu. Cautions to Users: 


cle 


All arguments in the main program must be 


J 


dimensioned the same as those in GANDPR. 


lon 


The main program should contain an BRROR TEST 


(see 2h.) 


Ce 


See attached flow chart depicting typical usage. 
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Fig. 111-2 Flow diagram showing 
typical usage of GANDPR 
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